#include "Steering Behavior.h"
#include "Entity Manager.h"
#include "Transformations.h"
#include "ExecutionLog.h"
#include "GameWorld.h"
#include "Defines.hpp"

#define CharacterSpeed 20

double SteeringBehavior::m_dWanderDistance = 2.0;
double SteeringBehavior::m_dWanderJitter = 80.0;
double SteeringBehavior::m_dWanderRadius = 1.0;

double SteeringBehavior::m_dWaypointSeekDistSq = 22*22;
double SteeringBehavior::m_dWeightSeek = 1.0;

double SteeringBehavior::m_dWeightFlee(1.0);
double SteeringBehavior::m_dWeightOffsetPursuit(1.0);
double SteeringBehavior::m_dWeightInterpose(1.0);
double SteeringBehavior::m_dWeightFollowPath(1.0);

double SteeringBehavior::m_dWeightObstacleAvoidance(10.0);
double SteeringBehavior::m_dWeightWander(1.0);
double SteeringBehavior::m_dWeightWallAvoidance(10.0);

SteeringBehavior::SteeringBehavior(MovingEntity* agent):
		m_pMovingEntity(agent),
		m_iFlags(0),
		m_dDboxLength(40.0),
		m_Feelers(3),
		m_Deceleration(normal),
		m_pTargetAgent1(NULL),
		m_pTargetAgent2(NULL)
{
	double theta = (rand()/RAND_MAX+1.0);
	m_vWanderTarget = Vector2D(m_dWanderRadius*cos(theta),
								m_dWanderRadius*sin(theta));
	m_pPath = NULL;
}

SteeringBehavior::~SteeringBehavior()
{
	if(m_pPath)
		NullDelete(m_pPath);
}

void SteeringBehavior::SetPath(Path* path)
{
	if(m_pPath != NULL)
		NullDelete(m_pPath);

	m_pPath = path;
}

Vector2D SteeringBehavior::Calculate()
{
	m_vSteeringForce.Zero();

	if(On(follow_path))
	{
		m_vSteeringForce += FollowPath() * m_dWeightFollowPath;
	}

	if(On(flee))
	{
		m_vSteeringForce += Flee(m_vTarget);
	}

	if(On(evade))
	{
		m_vSteeringForce += Evade(m_pTargetAgent1)*10.0;
	}

	if(On(wait))
	{
		m_vSteeringForce.Zero();
		m_pMovingEntity->Stop();
	}

	if(On(pursuit))
	{
		m_vSteeringForce += Pursuit(m_pTargetAgent1);
	}

	if(On(separation))
	{
		m_vSteeringForce += Separation(m_GroupNeighbors)*10.0; // Seperation weight
	}

	if(On(offset_pursuit))
	{
		m_vSteeringForce += OffsetPursuit(m_pTargetAgent1, m_vOffset)*10.0; // Offset Pursuit weight
	}

	if(On(arrive))
	{
		m_vSteeringForce += Arrive(m_vTarget, fast);
	}

	if(m_vSteeringForce.isZero())
	{
		m_pMovingEntity->Stop();
	}
	m_vSteeringForce.Truncate(CharacterSpeed);
	return m_vSteeringForce;
}



//------------------------------- Seek -----------------------------------
//
//  Given a target, this behavior returns a steering force which will
//  direct the agent towards the target
//------------------------------------------------------------------------
Vector2D SteeringBehavior::Seek(Vector2D TargetPos)
{
	Vector2D DesiredVelocity = Vec2DNormalize(TargetPos - m_pMovingEntity->Pos())
		* CharacterSpeed;

  return DesiredVelocity;
}

//--------------------------- Arrive -------------------------------------
//
//  This behavior is similar to seek but it attempts to arrive at the
//  target with a zero velocity
//------------------------------------------------------------------------
Vector2D SteeringBehavior::Arrive(Vector2D     TargetPos,
                                  Deceleration deceleration)
{
  Vector2D ToTarget = TargetPos - m_pMovingEntity->Pos();

  //calculate the distance to the target
  double dist = ToTarget.Length();

  if (dist > 0)
  {
    //because Deceleration is enumerated as an int, this value is required
    //to provide fine tweaking of the deceleration..
    const double DecelerationTweaker = 0.3;

    //calculate the speed required to reach the target given the desired
    //deceleration
    double speed =  dist / ((double).5 * DecelerationTweaker);

    //make sure the velocity does not exceed the max
    speed = min(speed, (double)CharacterSpeed );

    //from here proceed just like Seek except we don't need to normalize
    //the ToTarget vector because we have already gone to the trouble
    //of calculating its length: dist.
    Vector2D DesiredVelocity =  ToTarget * speed / dist;

    return (DesiredVelocity - m_pMovingEntity->GetVelocity());
  }

  return Vector2D(0,0);
}


// ------------------------ Follow Path ----------------------------
//
//
Vector2D SteeringBehavior::FollowPath()
{
	if(m_pPath != NULL)
	{
		if(m_pPath->IsValid())
		{


			if(Vec2DDistanceSq(m_pPath->CurrentWaypoint(), m_pMovingEntity->Pos()) < m_dWaypointSeekDistSq)
			{
				m_pPath->SetToNextWaypoint();
			}

			if(m_pPath->Finished())
			{
				this->FollowPathOff();
				return Vector2D();

				Vector2D newPos;
				newPos.x = floor(m_pMovingEntity->Pos().x /32 + .5)*32;
				newPos.y = floor(m_pMovingEntity->Pos().y /32 + .5)*32;

				this->m_pMovingEntity->SetPos(newPos);
			}

			if (m_pPath->CurrentWaypoint() != m_pPath->lastPoint())
			{
				return Seek(m_pPath->CurrentWaypoint());
			}
			else
			{
				return Arrive(m_pPath->CurrentWaypoint(), fast);
			}
		}
	}

	return Vector2D();
}

Vector2D SteeringBehavior::Flee(Vector2D TargetPos)
{

  Vector2D DesiredVelocity = Vec2DNormalize(m_pMovingEntity->Pos() - TargetPos)
                            * CharacterSpeed;

  return (DesiredVelocity - m_pMovingEntity->GetVelocity());
}

//----------------------------- Evade ------------------------------------
//
//  similar to pursuit except the agent Flees from the estimated future
//  position of the EvadeTarget
//------------------------------------------------------------------------
Vector2D SteeringBehavior::Evade(const MovingEntity* EvadeTarget)
{
  /* Not necessary to include the check for facing direction this time */

  Vector2D ToPursuer = EvadeTarget->Pos() - m_pMovingEntity->Pos();

  //uncomment the following two lines to have Evade only consider pursuers
  //within a 'threat range'
  const double ThreatRange = 100.0;
  if (ToPursuer.LengthSq() > ThreatRange * ThreatRange) return Vector2D();

  //the lookahead time is propotional to the distance between the EvadeTarget
  //and the EvadeTarget; and is inversely proportional to the sum of the
  //agents' velocities
  double LookAheadTime = ToPursuer.Length() /
                         (CharacterSpeed + CharacterSpeed);

  //now flee away from predicted future position of the EvadeTarget
  return Flee(EvadeTarget->Pos() + EvadeTarget->GetVelocity() * LookAheadTime);
}




//---------------------------- Separation --------------------------------
//
// this calculates a force repelling from the other neighbors
//------------------------------------------------------------------------
Vector2D SteeringBehavior::Separation(list<Character*> &neighbors)
{  
  Vector2D SteeringForce;

  for (list<Character*>::iterator iter = neighbors.begin(); iter != neighbors.end(); 
		iter++)
  {
    //make sure this agent isn't included in the calculations ***also make sure it doesn't
    //include the evade target ***
    if(((*iter)->ID() != m_pMovingEntity->ID()))
    {
      Vector2D ToAgent = m_pMovingEntity->Pos() - (*iter)->Pos();
	  if(ToAgent.Length() > m_pMovingEntity->CRadius()*4) // If they are in the neighborhood radius
	  {
		  //scale the force inversely proportional to the agents distance  
		  //from its neighbor.
		  SteeringForce += Vec2DNormalize(ToAgent)/ToAgent.Length();
	  }
    }
  }

  return SteeringForce;
}

//------------------------- Offset Pursuit -------------------------------
//
//  Produces a steering force that keeps a vehicle at a specified offset
//  from a leader vehicle
//------------------------------------------------------------------------
Vector2D SteeringBehavior::OffsetPursuit(MovingEntity*  leader,
                                          const Vector2D offset)
{
  //calculate the offset's position in world space
  Vector2D WorldOffsetPos = PointToWorldSpace(offset,
                                                  leader->Heading(),
                                                  leader->Side(),
                                                  leader->Pos());

  Vector2D ToOffset = WorldOffsetPos - m_pMovingEntity->Pos();

  //the lookahead time is propotional to the distance between the leader
  //and the pursuer; and is inversely proportional to the sum of both
  //agent's velocities
  double LookAheadTime = ToOffset.Length() / 
                        (CharacterSpeed + CharacterSpeed);
  
  //now Arrive at the predicted future position of the offset
  return Arrive(WorldOffsetPos + leader->GetVelocity() * LookAheadTime, fast);
}

Vector2D SteeringBehavior::Pursuit(MovingEntity* target)
{

  Vector2D ToTarget = target->Pos() - m_pMovingEntity->Pos();

  double RelativeHeading = m_pMovingEntity->Heading().Dot(target->Heading());

  if ( (ToTarget.Dot(m_pMovingEntity->Heading()) > 0) &&  
       (RelativeHeading < -0.95))  //acos(0.95)=18 degs
  {
    return Seek(target->Pos());
  }

  //Not considered ahead so we predict where the target will be.
 
  //the lookahead time is propotional to the distance between the target
  //and the pursuer; and is inversely proportional to the sum of the
  //agent's velocities
  double LookAheadTime = ToTarget.Length() / 
	  (/*m_pMovingEntity->MaxSpeed()*/CharacterSpeed + target->GetSpeed());
  
  //now seek to the predicted future position of the target
  return Seek(target->Pos() + target->GetVelocity() * LookAheadTime);
}


Vector2D SteeringBehavior::ObstacleAvoidance()
{
	return Vector2D();
}

void SteeringBehavior::DrawTarget()
{
	DrawPrimitives->DrawCircle(m_vTarget.x - ClipPlane->GetX(), m_vTarget.y - ClipPlane->GetY(), 20);
}